#include <math.h>

const static double pi = 3.1415926535897932384626;
const static double x_pi = 3.1415926535897932384626 * 3000.0 / 180.0;
const static double a = 6378245.0;
const static double ee = 0.00669342162296594323;

static double transformLat(double x, double y) {
    double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(fabs(x));
    ret = (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0 + ret;
    ret = (20.0 * sin(y * pi) + 40.0 * sin(y / 3.0 * pi)) * 2.0 / 3.0 + ret;
    ret = (160.0 * sin(y / 12.0 * pi) + 320.0 * sin(y * pi / 30.0)) * 2.0 / 3.0 + ret;
    return ret;
}

static double transformLon(double x, double y) {
    double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(fabs(x));
    ret = (20.0 * sin(6.0 * x * pi) + 20.0 * sin(2.0 * x * pi)) * 2.0 / 3.0 + ret;
    ret = (20.0 * sin(x * pi) + 40.0 * sin(x / 3.0 * pi)) * 2.0 / 3.0 + ret;
    ret = (150.0 * sin(x / 12.0 * pi) + 300.0 * sin(x / 30.0 * pi)) * 2.0 / 3.0 + ret;
    return ret;
}

void util_wgs84_gcj02(double wgs84_lat, double wgs84_lon, double *gcj02_lat, double *gcj02_lon) {
    double dLat = transformLat(wgs84_lon - 105.0, wgs84_lat - 35.0);
    double dLon = transformLon(wgs84_lon - 105.0, wgs84_lat - 35.0);
    double radLat = wgs84_lat / 180.0 * pi;
    double sinRadLat = sin(radLat);
    double magic = 1.0 - (ee * sinRadLat * sinRadLat);
    double sqrtMagic = sqrt(magic);
    dLat = dLat * 180.0 / ((a * (1.0 - ee)) / (magic * sqrtMagic) * pi);
    dLon = dLon * 180.0 / (a / sqrtMagic * cos(radLat) * pi);

    *gcj02_lat = wgs84_lat + dLat;
    *gcj02_lon = wgs84_lon + dLon;
}

void util_gcj02_wgs84(double gcj02_lat, double gcj02_lon, double *wgs84_lat, double *wgs84_lon) {
    double dlat = transformLat(gcj02_lon - 105.0, gcj02_lat - 35.0);
    double dlng = transformLon(gcj02_lon - 105.0, gcj02_lat - 35.0);
    double radlat = gcj02_lat / 180.0 * pi;
    double sinRadLat = sin(radlat);
    double magic = 1 - ee * sinRadLat * sinRadLat;
    double sqrtmagic = sqrt(magic);
    dlat = (dlat * 180.0) / ((a * (1.0 - ee)) / (magic * sqrtmagic) * pi);
    dlng = (dlng * 180.0) / (a / sqrtmagic * cos(radlat) * pi);
    *wgs84_lat = gcj02_lat * 2 - (gcj02_lat + dlat);
    *wgs84_lon = gcj02_lon * 2 - (gcj02_lon + dlng);
}

void util_bd09_gcj02(double bd09_lat, double bd09_lon, double *gcj02_lat, double *gcj02_lon) {
    double x = bd09_lon - 0.0065;
    double y = bd09_lat - 0.006;
    double z = sqrt(x * x + y * y) - 0.00002 * sin(y * x_pi);
    double theta = atan2(y, x) - 0.000003 * cos(x * x_pi);
    *gcj02_lon = z * cos(theta);
    *gcj02_lat = z * sin(theta);
}

void util_gcj02_bd09(double gcj02_lat, double gcj02_lon, double *bd09_lat, double *bd09_lon) {
    double z = sqrt(gcj02_lon * gcj02_lon + gcj02_lat * gcj02_lat) + 0.00002 * sin(gcj02_lat * x_pi);
    double theta = atan2(gcj02_lat, gcj02_lon) + 0.000003 * cos(gcj02_lon * x_pi);
    *bd09_lon = z * cos(theta) + 0.0065;
    *bd09_lat = z * sin(theta) + 0.006;
}